#include <Servo.h>

Servo one;
Servo two;
Servo three;
Servo four;
int msOne = 1500;
int msTwo = 1500;
int msThree = 1500;
int msFour = 1500;
String inputString = "";
boolean stringComplete = false;

void setup(){
  one.attach(2);
  one.writeMicroseconds(msOne);

  two.attach(3);
  two.writeMicroseconds(msTwo);

  three.attach(4);
  three.writeMicroseconds(msThree);

  four.attach(5);
  four.writeMicroseconds(msFour);

  Serial.begin(115200);
  inputString.reserve(200);
}

void loop(){
  if(stringComplete){
    Serial.println("wrote");
    one.writeMicroseconds(msOne);
    two.writeMicroseconds(msTwo);
    three.writeMicroseconds(msThree);
    four.writeMicroseconds(msFour);
    stringComplete = false;
  }
}

void serialEvent(){
  while(Serial.available()){
    char inChar = (char)Serial.read();
    inputString += inChar;
    if(inChar == '\n'){
      Serial.println(inputString);
      char buff[inputString.length()];
      inputString.toCharArray(buff, inputString.length());
      char num = buff[0];
      
      char command[inputString.length() - 1];
      for(int i = 1 ; i < inputString.length(); ++i){
      	      command[i-1] = buff[i];
      }     

      int intform = atoi(command);
      Serial.println(intform);
      int ms = constrain(intform, 500, 2500);
      Serial.println(ms);

      if(num == '1'){
      	    msOne = ms;
	    Serial.println("one");
      }else if(num == '2'){
      	    msTwo = ms;
	    Serial.println("two");
      }else if(num == '3'){
      	    msThree = ms;
	    Serial.println("three");
      }else if(num == '4'){
      	    msFour = ms;
	    Serial.println("four");
      }


      stringComplete = true;
      inputString = "";
    }
  }
}
